/*
 *   Copyright (C) 2011 David Swords
 *	
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

package motion;

import java.util.Calendar;

import math.Measurement;
import sensor.SensorReader;
import comm.Communicator;
import format.NumberConverter;

public class MotionController {

	private final String class_details = "MotionController.";
	private Communicator comm;
	private SensorReader sens;
	private Measurement meas;
	private NumberConverter numb;
	private final int MAX_SPEED = 50;
	
	public MotionController(String IP) {
		final String constructor_details = class_details + "MotionController(String)";
		chk(IP.isEmpty(), "Given IP was blank.", constructor_details);
		comm  = new Communicator(IP);
		sens = new SensorReader(IP);
		meas = new Measurement();
		numb = new NumberConverter();
		chk(comm == null, "Communicator is equal null, please initialize.", constructor_details);
		chk(sens == null, "SensorReader is equal null, please initialize.", constructor_details);
		chk(meas == null, "Measurement is equal null, please initialize.", constructor_details);
		chk(numb == null, "NumberConverter is equal null, please initialize.", constructor_details);
		comm.connect();
	}
	
	private void chk(final boolean expression, final String message, final String location) {
		if(expression) {
			System.err.println("#############################################################");
			System.err.println(message);
			System.err.println(location);
			System.err.println("#############################################################");
			System.exit(1);
		}
	}
	
	public void forward() {
		final String method_details = class_details + "forward() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("s xx x108 x0002 " + MAX_SPEED + " " + MAX_SPEED + " x0000 \n");
	}
	
	public void forward_d(double distance) {
		final String method_details = class_details + "forward(double) returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(sens == null, "SensorReader is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		chk(!sens.isConnected(), "SensorReader is not connected.", method_details);
		chk(distance < 0, "Negative distance value given, cannot comply.", method_details);
		
		double x_start = sens.pose().x();
		double y_start = sens.pose().y();
		
		if(distance != 0) {
			while(meas.euclideanDistance(x_start, y_start, sens.pose().x(), sens.pose().y()) <= distance) {
				
				forward();
				
				try {
					Thread.sleep(300);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				
				System.out.println(x_start + " " + y_start + " " + sens.pose().x() + " " + sens.pose().y() + " " + distance + " " + meas.euclideanDistance(x_start, y_start, sens.pose().x(), sens.pose().y()));
				
				stop();
				
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
		}
	}
	
	public void forward_t(long time) {
		final String method_details = class_details + "forward(double) returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(sens == null, "SensorReader is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		chk(!sens.isConnected(), "SensorReader is not connected.", method_details);
		chk(time < 0, "Negative time value given, cannot comply.", method_details);
		
		Calendar cal = Calendar.getInstance();
		long s_time = cal.getTimeInMillis();
		long e_time = s_time + time * 1000;
		
		while(s_time < e_time) {
			cal = Calendar.getInstance();
			s_time = cal.getTimeInMillis();
			forward();
		}
		stop();
	}
	
	public void backward() {
		final String method_details = class_details + "backward() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("s xx x108 x0002 -" + MAX_SPEED + " -" + MAX_SPEED + " x0000 \n");
	}
	
	public void backward(double distance) {
		final String method_details = class_details + "backward(int) returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(sens == null, "SensorReader is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		chk(!sens.isConnected(), "SensorReader is not connected.", method_details);
		chk(distance < 0, "Negative distance value given, cannot comply.", method_details);
		
		double x_start = sens.pose().x();
		double y_start = sens.pose().y();
		
		for(int i = 0; meas.euclideanDistance(x_start, y_start, sens.pose().x(), sens.pose().y()) <= distance; i++) {
			backward();
		}
	}
	
	public void right() {
		final String method_details = class_details + "right() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("s xx x108 x0003 " + MAX_SPEED + " -" + MAX_SPEED + " x0000 \n");
	}
	
	public void left() {
		final String method_details = class_details + "left() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("s xx x108 x0003 -" + MAX_SPEED + " " + MAX_SPEED + " x0000 \n");
	}
	
	public void direction(double radian) {
		final String method_details = class_details + "direction(double) returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(sens == null, "SensorReader is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		chk(!sens.isConnected(), "SensorReader is not connected.", method_details);
		chk(!(radian >= 0 && radian <= 2*Math.PI), "MotionController.direction(double) returns void", method_details);		

		while(!(radian + 0.07 > sens.pose().t() && radian - 0.07 < sens.pose().t())) {	
			while(!(radian + 0.07 > sens.pose().t())) {
				left();
				
				try {
					Thread.sleep(300);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				
				stop();
				
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			
			while(!(radian - 0.07 < sens.pose().t())) {
				right();
				
				try {
					Thread.sleep(300);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				
				stop();
				
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
		}
	}
	
	public void flipperClock() {
		final String method_details = class_details + "flipperClock() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("w xx x10c 2 x10d -100 0 \n");
	}
	
	public void flipperAntiClock() {
		final String method_details = class_details + "flipperAntiClock() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("w xx x10c 2 x10d 100 0 \n");
	}
	
	public void flipperPosition(double radian) {
		final String method_details = class_details + "flipperPosition(double) returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		chk(!(radian >= 0 && radian <= 2*Math.PI), "MotionController.direction(double) returns void", method_details);		
		Calendar cal = Calendar.getInstance();
		long s_time = cal.getTimeInMillis();
		long e_time = s_time + 1000;
		
		while(s_time < e_time) {
			cal = Calendar.getInstance();
			s_time = cal.getTimeInMillis();
			comm.write("w 010 x10c 3 x10d " + numb.radianToFlipperSystem(radian) + " 0 \n");
		}
	}
	
	public void flipperReset() {
		final String method_details = class_details + "flipperReset() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("w 010 x10c 3 x10d 00000 0 \n");
	}
	
	public void stop() {
		final String method_details = class_details + "stop() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.write("w xx x0108 x0000 x10c x0000 0 \n");
	}
	
	public boolean isConnected() {
		final String method_details = class_details + "isConnected() returns boolean";
		chk(comm == null, "Communicator is equal to null, please initialize.", method_details);
		return comm.isConnected();
	}
	
	public void disconnect() {
		final String method_details = class_details + "disconnect() returns void";
		chk(comm == null, "Communicator is equal null, please initialize.", method_details);
		chk(!comm.isConnected(), "Communicator is not connected.", method_details);
		comm.disconnect();
	}
}
